/*
* Copyright (c) 2013 Google, Inc.
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#include "b2_voronoi_diagram.h"
#include "b2_stack_queue.h"
#include "box2d/b2_collision.h"

b2VoronoiDiagram::b2VoronoiDiagram(
  b2StackAllocator* allocator, int32 generatorCapacity)
{
  m_allocator = allocator;
  m_generatorBuffer =
    (Generator*) allocator->Allocate(
      sizeof(Generator) * generatorCapacity);
  m_generatorCapacity = generatorCapacity;
  m_generatorCount = 0;
  m_countX = 0;
  m_countY = 0;
  m_diagram = NULL;
}

b2VoronoiDiagram::~b2VoronoiDiagram()
{
  if (m_diagram)
  {
    m_allocator->Free(m_diagram);
  }
  m_allocator->Free(m_generatorBuffer);
}

void b2VoronoiDiagram::AddGenerator(
  const b2Vec2& center, int32 tag, bool necessary)
{
  b2Assert(m_generatorCount < m_generatorCapacity);
  Generator& g = m_generatorBuffer[m_generatorCount++];
  g.center = center;
  g.tag = tag;
  g.necessary = necessary;
}

void b2VoronoiDiagram::Generate(float32 radius, float32 margin)
{
  b2Assert(m_diagram == NULL);
  float32 inverseRadius = 1 / radius;
  b2Vec2 lower(+b2_maxFloat, +b2_maxFloat);
  b2Vec2 upper(-b2_maxFloat, -b2_maxFloat);
  for (int32 k = 0; k < m_generatorCount; k++)
  {
    Generator& g = m_generatorBuffer[k];
    if (g.necessary)
    {
      lower = b2Min(lower, g.center);
      upper = b2Max(upper, g.center);
    }
  }
  lower.x -= margin;
  lower.y -= margin;
  upper.x += margin;
  upper.y += margin;
  m_countX = 1 + (int32) (inverseRadius * (upper.x - lower.x));
  m_countY = 1 + (int32) (inverseRadius * (upper.y - lower.y));
  m_diagram = (Generator**)
    m_allocator->Allocate(sizeof(Generator*) * m_countX * m_countY);
  for (int32 i = 0; i < m_countX * m_countY; i++)
  {
    m_diagram[i] = NULL;
  }
  // (4 * m_countX * m_countY) is the queue capacity that is experimentally
  // known to be necessary and sufficient for general particle distributions.
  b2StackQueue<b2VoronoiDiagramTask> queue(
    m_allocator, 4 * m_countX * m_countY);
  for (int32 k = 0; k < m_generatorCount; k++)
  {
    Generator& g = m_generatorBuffer[k];
    g.center = inverseRadius * (g.center - lower);
    int32 x = (int32) g.center.x;
    int32 y = (int32) g.center.y;
    if (x >=0 && y >= 0 && x < m_countX && y < m_countY)
    {
      queue.Push(b2VoronoiDiagramTask(x, y, x + y * m_countX, &g));
    }
  }
  while (!queue.Empty())
  {
    int32 x = queue.Front().m_x;
    int32 y = queue.Front().m_y;
    int32 i = queue.Front().m_i;
    Generator* g = queue.Front().m_generator;
    queue.Pop();
    if (!m_diagram[i])
    {
      m_diagram[i] = g;
      if (x > 0)
      {
        queue.Push(b2VoronoiDiagramTask(x - 1, y, i - 1, g));
      }
      if (y > 0)
      {
        queue.Push(b2VoronoiDiagramTask(x, y - 1, i - m_countX, g));
      }
      if (x < m_countX - 1)
      {
        queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, g));
      }
      if (y < m_countY - 1)
      {
        queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, g));
      }
    }
  }
  for (int32 y = 0; y < m_countY; y++)
  {
    for (int32 x = 0; x < m_countX - 1; x++)
    {
      int32 i = x + y * m_countX;
      Generator* a = m_diagram[i];
      Generator* b = m_diagram[i + 1];
      if (a != b)
      {
        queue.Push(b2VoronoiDiagramTask(x, y, i, b));
        queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, a));
      }
    }
  }
  for (int32 y = 0; y < m_countY - 1; y++)
  {
    for (int32 x = 0; x < m_countX; x++)
    {
      int32 i = x + y * m_countX;
      Generator* a = m_diagram[i];
      Generator* b = m_diagram[i + m_countX];
      if (a != b)
      {
        queue.Push(b2VoronoiDiagramTask(x, y, i, b));
        queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, a));
      }
    }
  }
  while (!queue.Empty())
  {
    const b2VoronoiDiagramTask& task = queue.Front();
    int32 x = task.m_x;
    int32 y = task.m_y;
    int32 i = task.m_i;
    Generator* k = task.m_generator;
    queue.Pop();
    Generator* a = m_diagram[i];
    Generator* b = k;
    if (a != b)
    {
      float32 ax = a->center.x - x;
      float32 ay = a->center.y - y;
      float32 bx = b->center.x - x;
      float32 by = b->center.y - y;
      float32 a2 = ax * ax + ay * ay;
      float32 b2 = bx * bx + by * by;
      if (a2 > b2)
      {
        m_diagram[i] = b;
        if (x > 0)
        {
          queue.Push(b2VoronoiDiagramTask(x - 1, y, i - 1, b));
        }
        if (y > 0)
        {
          queue.Push(b2VoronoiDiagramTask(x, y - 1, i - m_countX, b));
        }
        if (x < m_countX - 1)
        {
          queue.Push(b2VoronoiDiagramTask(x + 1, y, i + 1, b));
        }
        if (y < m_countY - 1)
        {
          queue.Push(b2VoronoiDiagramTask(x, y + 1, i + m_countX, b));
        }
      }
    }
  }
}

void b2VoronoiDiagram::GetNodes(NodeCallback& callback) const
{
  for (int32 y = 0; y < m_countY - 1; y++)
  {
    for (int32 x = 0; x < m_countX - 1; x++)
    {
      int32 i = x + y * m_countX;
      const Generator* a = m_diagram[i];
      const Generator* b = m_diagram[i + 1];
      const Generator* c = m_diagram[i + m_countX];
      const Generator* d = m_diagram[i + 1 + m_countX];
      if (b != c)
      {
        if (a != b && a != c &&
          (a->necessary || b->necessary || c->necessary))
        {
          callback(a->tag, b->tag, c->tag);
        }
        if (d != b && d != c &&
          (b->necessary || d->necessary || c->necessary))
        {
          callback(b->tag, d->tag, c->tag);
        }
      }
    }
  }
}
